﻿using System;
using SlimDX;
using SlimDX.Direct3D9;

namespace Splino
{
    public static class SplinoMath
    {
        private const float offset = 0.000001f;

        /// <summary>
        /// Transforms a vector from one coordinates system into another
        /// </summary>
        /// <param name="source">The vector to transform</param>
        /// <param name="sourceOrigin">The location of the source coordinate system origin</param>
        /// <param name="sourceUnitX">The X direction of the source coordinate system</param>
        /// <param name="sourceUnitY">The Y direction of the source coordinate system</param>
        /// <param name="sourceUnitZ">The Z direction of the source coordinate system</param>
        /// <param name="targetOrigin">The location of the target coordinate system origin</param>
        /// <param name="targetUnitX">The X direction of the target coordinate system</param>
        /// <param name="targetUnitY">The Y direction of the target coordinate system</param>
        /// <param name="targetUnitZ">The Z direction of the target coordinate system</param>
        /// <returns>The transformed vector</returns>
        public static Vector3 ConvertBetweenCoordinateSystems(
            Vector3 source,
            Vector3 sourceOrigin,
            Vector3 sourceUnitX,
            Vector3 sourceUnitY,
            Vector3 sourceUnitZ,
            Vector3 targetOrigin,
            Vector3 targetUnitX,
            Vector3 targetUnitY,
            Vector3 targetUnitZ)
        {
            return ConvertFromWorldCoordinateSystem(
                ConvertToWorldCoordinateSystem(
                    source,
                    sourceOrigin,
                    sourceUnitX,
                    sourceUnitY,
                    sourceUnitZ),
                targetOrigin,
                targetUnitX,
                targetUnitY,
                targetUnitZ);
        }

        /// <summary>
        /// Transforms a vector from the defined coordinate system into world coordinates
        /// </summary>
        /// <param name="source">The vector tor transform</param>
        /// <param name="sourceOrigin">The location of the source coordinate system origin</param>
        /// <param name="sourceUnitX">The X direction of the source coordinate system</param>
        /// <param name="sourceUnitY">The Y direction of the source coordinate system</param>
        /// <param name="sourceUnitZ">The Z direction of the source coordinate system</param>
        /// <returns>The transformed vector</returns>
        public static Vector3 ConvertToWorldCoordinateSystem(
            Vector3 source,
            Vector3 sourceOrigin,
            Vector3 sourceUnitX,
            Vector3 sourceUnitY,
            Vector3 sourceUnitZ)
        {
            return sourceOrigin + sourceUnitX * source.X + sourceUnitY * source.Y + sourceUnitZ * source.Z;
        }

        /// <summary>
        /// Transformed a vector defined in world coordinates into the defined coordinate system
        /// </summary>
        /// <param name="source">The vector tor transform</param>
        /// <param name="targetOrigin">The location of the target coordinate system origin</param>
        /// <param name="targetUnitX">The X direction of the target coordinate system</param>
        /// <param name="targetUnitY">The Y direction of the target coordinate system</param>
        /// <param name="targetUnitZ">The Z direction of the target coordinate system</param>
        /// <returns>The transformed vector</returns>
        public static Vector3 ConvertFromWorldCoordinateSystem(
            Vector3 source,
            Vector3 targetOrigin,
            Vector3 targetUnitX,
            Vector3 targetUnitY,
            Vector3 targetUnitZ)
        {
            Vector3 sourceFromTargetOrigin = source - targetOrigin;
            float xComp = Vector3.Dot(sourceFromTargetOrigin, Vector3.Normalize(targetUnitX)) / targetUnitX.Length();
            float yComp = Vector3.Dot(sourceFromTargetOrigin, Vector3.Normalize(targetUnitY)) / targetUnitY.Length();
            float zComp = Vector3.Dot(sourceFromTargetOrigin, Vector3.Normalize(targetUnitZ)) / targetUnitZ.Length();
            return new Vector3(xComp, yComp, zComp);
        }

        /// <summary>
        /// Returns the component wise absolute value of a vector
        /// </summary>
        /// <param name="v"></param>
        /// <returns></returns>
        public static Vector3 AbsVector3(Vector3 v)
        {
            return new Vector3(Math.Abs(v.X), Math.Abs(v.Y), Math.Abs(v.Z));
        }


        public static Nullable<Vector3> RayTriangleIntersect(
            Ray ray,
            Vector3 triangleVertex1,
            Vector3 triangleVertex2,
            Vector3 triangleVertex3)
        {
            float dist;
            return RayTriangleIntersect(ray, triangleVertex1, triangleVertex2, triangleVertex3, out dist);
        }

        public static Nullable<Vector3> RayTriangleIntersect(
            Ray ray,
            Vector3 triangleVertex1,
            Vector3 triangleVertex2,
            Vector3 triangleVertex3,
            out float distance)
        {
            distance = -1;
            Vector3 edge1 = triangleVertex2 - triangleVertex1;
            Vector3 edge2 = triangleVertex3 - triangleVertex1;

            Vector3 pvec = Vector3.Cross(ray.Direction, edge2);
            float det = Vector3.Dot(edge1, pvec);

            if (det > -offset && det < offset)
                return null;

            float invDet = 1.0f / det;

            Vector3 tvec = ray.Position - triangleVertex1;
            float u = Vector3.Dot(tvec, pvec) * invDet;
            if (u < 0 || u > 1)
                return null;
            Vector3 qvec = Vector3.Cross(tvec, edge1);
            float v = Vector3.Dot(ray.Direction, qvec) * invDet;
            if (v < 0 || u + v > 1)
                return null;
            distance = Vector3.Dot(edge2, qvec) * invDet;
            return new Nullable<Vector3>(distance * ray.Direction + ray.Position);
        }


        /// <summary>
        /// Creates a rotation quaternion based on vectors defining a forward and up direction
        /// </summary>
        /// <param name="direction"></param>
        /// <param name="up"></param>
        /// <returns></returns>
        public static Quaternion RotationQuatFromVectors(Vector3 forward, Vector3 up)
        {
            Quaternion quat = new Quaternion();
            Vector3 cross = Vector3.Cross(forward, up);
            quat.X = cross.X;
            quat.Y = cross.Y;
            quat.Z = cross.Z;
            quat.W = (float)Math.Sqrt(forward.LengthSquared() * up.LengthSquared()) + Vector3.Dot(forward, up);
            quat.Normalize();
            return quat;
        }

        /// <summary>
        /// Creates a rotation matrix based on vectors defining forward and up direction
        /// </summary>
        /// <param name="forward"></param>
        /// <param name="up"></param>
        /// <returns></returns>
        public static Matrix RotationMatrixFromVectors(Vector3 forward, Vector3 up)
        {
            Vector3 right = Vector3.Cross(forward, up);
            Matrix rotationMatrix = new Matrix();
            rotationMatrix.M11 = right.X;
            rotationMatrix.M12 = right.Y;
            rotationMatrix.M12 = right.Z;
            rotationMatrix.M12 = 0;

            rotationMatrix.M12 = up.X;
            rotationMatrix.M12 = up.Y;
            rotationMatrix.M12 = up.Z;
            rotationMatrix.M12 = 0;

            rotationMatrix.M12 = forward.X;
            rotationMatrix.M12 = forward.Y;
            rotationMatrix.M12 = forward.Z;
            rotationMatrix.M12 = 0;

            rotationMatrix.M12 = 0;
            rotationMatrix.M12 = 0;
            rotationMatrix.M12 = 0;
            rotationMatrix.M12 = 1;

            return rotationMatrix;
        }

        public static Vector3 IntersectionOf3DLines(Vector3 p1, Vector3 v1, Vector3 p2, Vector3 v2)
        {
            float p1x = p1.X;
            float p1y = p1.Y;
            float p1z = p1.Z;
            float v1x = v1.X;
            float v1y = v1.Y;
            float v1z = v1.Z;
            float p2x = p2.X;
            float p2y = p2.Y;
            float p2z = p2.Z;
            float v2x = v2.X;
            float v2y = v2.Y;
            float v2z = v2.Z;

            float txy = (p2y - p1y - ((v1y * p2x) / v1x) + ((v1y * p1x) / v1x)) / (v2y - (v1y * v2x) / v1x);
            float txz = (p2z - p1z - ((v1z * p2x) / v1x) + ((v1z * p1x) / v1x)) / (v2z - (v1z * v2x) / v1x);
            float tyz = (p2z - p1z - ((v1z * p2y) / v1y) + ((v1z * p1y) / v1y)) / (v2z - (v1z * v2y) / v1y);

            Vector3 intxy = p2 + txy * v2;

            return intxy;
        }
    }
}
